from collections import deque
import numpy as np
from Hardware.BaseFtSensor import BaseFt
import struct
import threading
import time
import serial

class ftsensor(BaseFt):
    def __init__(self,setZeroForce:bool=False):
        '''
        setZeroForce:是否需要将读数清零零点
        '''
        #region 串口初始化，传感器开始读取数据
        BAUDRATE = 460800
        BYTESIZE = 8
        PARITY = "N"
        STOPBITS = 1
        TIMEOUT = 1
        PORTNAME = "COM9"
        self.ser = serial.Serial(port=PORTNAME, baudrate=BAUDRATE, bytesize=BYTESIZE, parity=PARITY, stopbits=STOPBITS,
                                timeout=TIMEOUT)
        command = bytes([0x48, 0xAA, 0x0D, 0x0A])
        self.ser.write(command)
        self.ser.flush()
        #endregion

        self.count = 0
        self.startTime = time.time()

        #region 接收数据
        self.isRunning = True
        self.setZeroForce = setZeroForce
        self.forceTorque = [0, 0, 0, 0, 0, 0]
        self.zeroForceReference = [0, 0, 0, 0, 0, 0]
        self.forcefillter = deque([[0, 0, 0, 0, 0, 0]] * 10, maxlen=10)
        self.thread = threading.Thread(target=self.start)
        self.thread.start()
        #endregion
        time.sleep(1)
        print("FT init")


    def start(self):
        buffer = b''  # 初始化缓冲区

        while self.isRunning:
            byte = self.ser.read(28)  # 读取一组数据
            if byte:
                buffer += byte  # 将字节添加到缓冲区

                # 检查缓冲区中是否存在完整帧
                while len(buffer) >= 28:  # 缓冲区至少要有28字节
                    # 查找起始标志
                    start_idx = buffer.find(b'\x48\xAA')

                    if start_idx == -1:
                        # 如果没有找到起始标志，丢弃无效数据
                        buffer = b''
                        break

                    # 如果找到了起始标志，但剩余数据不足以形成一个完整的帧，等待更多数据
                    if len(buffer) - start_idx < 28:
                        break

                    # 检查帧结束标志
                    potential_frame = buffer[start_idx:start_idx + 28]
                    if potential_frame[-2:] == b'\x0D\x0A':
                        # 找到有效帧
                        # hex_string = ' '.join(f'{byte:02x}' for byte in potential_frame)
                        self.forceTorque = self._process_frame(potential_frame)

                    # 如果不是有效帧，移除起始标志之前的数据，继续查找
                    buffer = buffer[start_idx + 1:]

    def _process_frame(self, frame):
        data=struct.unpack('<6f', frame[2:26])   # 将字节数据转换为浮点数，输出为长度为6的元组，对应六维FT数据
        if self.setZeroForce:
            self.zeroForceReference = list(data)
            self.setZeroForce = False
        self.forceTorque = [x - y for x, y in zip(list(data), self.zeroForceReference)]
        self.forcefillter.append(self.forceTorque)

        #print the frequency
        # self.count += 1
        # if self.count % 1000 == 0:
        #     print(f"frequency:{self.count / (time.time() - self.startTime)}Hz")

    def GetForce(self):
        avgforce = np.mean(self.forcefillter, axis=0)
        avgforce = np.round(avgforce, 2)
        return avgforce

    def FtZero(self):
        self.setZeroForce = True

    def DisConnect(self):
        self.isRunning = False

if __name__ == "__main__":
    import sys

    sensor = ftsensor(True)
    try:
        start_time = time.time()
        while time.time() - start_time < 60:
            # print(f"\r{sensor.GetForce()}")
            sys.stdout.write(f"\r{sensor.GetForce()}")
            sys.stdout.flush()

        sensor.DisConnect()
    except KeyboardInterrupt:
        sensor.DisConnect()
        print("exit")